Add inverse for CartesianPose and define operators explicitely #158

domire8 · 2021-06-18T12:03:12Z

Sooo issue #156 made us think about the operators a bit more and we came to the conclusion that

The resulting type is defined by the right operand (e.g. pose * twist is a twist)
twist and wrench do not make sense as left operands

That said, I deleted the * operators for twist and wrench, and added explicit * operators for the pose.

I expect that this might change again during this PR, which is why I'm waiting with the CHANGELOG and pybindings.

eeberhard · 2021-06-18T12:33:23Z

source/state_representation/src/space/cartesian/CartesianPose.cpp

@@ -123,6 +135,25 @@ Eigen::VectorXd CartesianPose::data() const {
  return this->get_pose();
 }

+CartesianPose CartesianPose::inverse() const {


why not use the CartesianState::inverse() and then downcast the return back to a pose? it would prevent some code duplication, and I would say the few extra linear operations are not hugely costly. It's also how the other operators are working anyway

eeberhard · 2021-06-18T12:35:06Z

source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp

+   * @brief Compute the inverse of the current CartesianPose
+   * @return the inverse corresponding to b_S_f (assuming this is f_S_b)
+   */
+  CartesianPose inverse() const;


The CartesianState inverse() const is not deleted in this header, which means this new declaration is hiding the base method. It would be safer to delete the base inverse method in this header as well.

If I add the line

CartesianState inverse() const = delete;

to the CartesianPose header, I cannot build state_representation anymore

Ah ok, my bad. Then instead you should mark the CartesianState::inverse as virtual and explicitly override the derived version here.

// in CartesianState.hpp virtual CartesianState CartesianState::inverse() const; // in CartesianPose.hpp CartesianPose CartesianPose::inverse() const override;

This should work because the return type of the derived function is a co-variant (the derived class) of the base function (the base class).

No, that doesn't work unfortunately... Even though it should...

Actually, there are other functions like that (the copy one) that are not defined virtual or deleted but that hide the CartesianState function

eeberhard · 2021-06-18T12:37:04Z

source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp

@@ -47,6 +47,9 @@ class CartesianTwist : public CartesianState {
  void set_force(const Eigen::Vector3d& force) = delete;
  void set_torque(const Eigen::Vector3d& torque) = delete;
  void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) = delete;
+  CartesianState inverse() const = delete;


Contrary to what @buschbapti mentioned on slack, I believe the inverse function still makes sense for the twist and the wrench. In the absence of any pose orientation, a pure twist inverse would simply be the negation of the linear and angular vectors (plus swapping the reference frame). But, as with the pose, you can still call the base CartesianState::inverse and downcast the return.

eeberhard · 2021-06-18T12:42:33Z

source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp

-   * @param twist CartesianTwist to multiply with
-   * @return the current CartesianTwist multiplied by the CartesianTwist given in argument
-   */
-  CartesianTwist& operator*=(const CartesianTwist& twist);


If we want to avoid another major version increment to control libraries 4.0, then the correct approach would be to mark these function as deprecated instead of deleting them:

[[deprecated]] CartesianTwist& operator*=(const CartesianTwist& twist);

Same goes for the base functions (e.g. CartesianState operator*(const CartesianState& state)), instead of deleting you would override / hide the implementation and mark it as deprecated. (hiding in this context is ok, since we don't intend the method to actually be used)

We could then remove these cases entirely the next time we decide to release a new major version.

I get the first part but not the second one. Do I have to explicitly add the operator CartesianState operator*(const CartesianState& state) in the header and source (where i just call the CartesianState operator) and then mark them as deprecated in the header?

yes exactly, for the second part you would have to re-implement those functions for this class. It may be more effort than it is worth, that's up to you and @buschbapti to decide. The first part I think still makes sense though.

We'll see if the tests pass without, otherwise it may be necessary.

domire8 · 2021-06-18T15:20:19Z

I marked the operators as deprecated like you proposed. However, the inverse functions now hide the one of the base class. I can't say if this a good solution or not but i think it's better than before.

domire8 · 2021-06-19T07:18:25Z

To be frank, it was maybe not our best idea to go down this road but it's always good to think about those things in detail. Here are my propositions:

If we are to follow the rule that the right operand type decides what the resulting type is, we should drop the *= operator because it only makes sense for statestate and posepose and we'd have to mark it as deprecated for the other 14 combinations.
For the * operator, only the state and the pose make sense as left operators. I marked all the other combinations as deprecated.

We can also leave this here and come back to it when you guys have more time to think about it. And if we go through with it we remove the operators with the next major version release.

eeberhard

I think this is the right road to go down, even if it's a little messy in the interim. By first explicitly marking out everything as deprecated, I think we can get a good idea of the implications without committing to a breaking change right away.

I agree with the overall rules that have emerged for product operations, which I will restate again for the record:

The return type of a product operation is the same as the right operand
The left operand of a product operation can only be CartesianPose or CartesianState

I'm going to give this one more thorough pass before approving but I appreciate the effort and the design direction.

eeberhard · 2021-06-19T08:49:53Z

source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp

@@ -148,7 +150,7 @@ class CartesianPose : public CartesianState {
   * @param pose CartesianPose to multiply with
   * @return the current CartesianPose multiplied by the CartesianPose given in argument
   */
-  CartesianPose& operator*=(const CartesianPose& pose);
+  [[deprecated]] CartesianPose& operator*=(const CartesianPose& pose);


These pose *= pose and state *= state operators are the ones that actually do make sense, as you mentioned in your comment - so why have you deprecated it here?

If I undersrand correctly we have two choices for the *= operator. Either we delete it completely or we have it in CartesianState, override it in CartesianPose and delete it in Wrench and Twist. And i was going for the first option hehe

well I would prefer the second option though *= for pose and state make sense so I would keep them and just delete them in the velocity and wrench

Yeah but let's face it then, either we have breaking changes or we postpone the *= operator until the next major release. Otherwise I have to implement every combination and mark them as deprecated instead of just deleting them where I can

buschbapti

This seems to go in the right direction thanks for that.

buschbapti · 2021-06-20T20:43:56Z

source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp

@@ -379,7 +379,7 @@ class CartesianState : public SpatialState {
   * @param state the state to compose with corresponding to b_S_c
   * @return the CartesianState corresponding f_S_c = f_S_b * b_S_c (assuming this is f_S_b)
   */
-  CartesianState& operator*=(const CartesianState& state);
+  [[deprecated]] CartesianState& operator*=(const CartesianState& state);


same comment as for the pose I would actually keep that one

buschbapti · 2021-06-20T20:46:09Z

source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp

+   * @param state CartesianPose to multiply with
+   * @return the current CartesianTwist multiplied by the CartesianPose given in argument
+   */
+  [[deprecated]] CartesianPose operator*(const CartesianPose& pose) const;


why do you need to deprecate this one? It was actually never implemented explicitly before. I think you can just deprecate the one with state and leave off the other ones otherwise you introduce also some forward declaration for wrench that is not needed

ok I think I understand that you need those to mark them as removed, as you don't want to deprecate the operator* which will be called if those are not implemented. Is that correct? Then yes I guess no other choices and forget the other comments.

Yes indeed, thats what you also said, if we explicitly implement one, we need to do all of them :)

buschbapti · 2021-06-20T20:46:24Z

source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp

+   * @param state CartesianWrench to multiply with
+   * @return the current CartesianTwist multiplied by the CartesianWrench given in argument
+   */
+  [[deprecated]] CartesianWrench operator*(const CartesianWrench& wrench) const;


same as above

buschbapti · 2021-06-20T20:47:14Z

source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp


 namespace state_representation {
+class CartesianPose;
+class CartesianTwist;


same as for the twist so I don't think this is needed

buschbapti · 2021-06-20T20:49:28Z

source/state_representation/src/space/cartesian/CartesianPose.cpp

@@ -123,6 +135,11 @@ Eigen::VectorXd CartesianPose::data() const {
  return this->get_pose();
 }

+CartesianPose CartesianPose::inverse() const {
+  CartesianPose result = this->CartesianState::inverse();


you can use the same writing in one liner as above in the operator*. I assume the compiler would then cast correctly as you specify the return type

buschbapti · 2021-06-20T20:50:58Z

source/state_representation/src/space/cartesian/CartesianPose.cpp

@@ -144,6 +161,10 @@ std::ostream& operator<<(std::ostream& os, const CartesianPose& pose) {
  return os;
 }

+CartesianPose operator*(const CartesianState& state, const CartesianPose& pose) {
+  return CartesianPose(state.operator*(pose));


same as above. If the compiler does the casting for you then no need to explicitly specify it. Otherwise you need to specify it in the other operator* for consistency.

buschbapti · 2021-06-20T20:55:05Z

source/state_representation/src/space/cartesian/CartesianState.cpp

+  return (*this);
+}
+
+CartesianState CartesianState::operator*(const CartesianState& state) const {


I am not sure why you changed that here. So basically, the previous version was defining this in operator*=, then calling it in this operator*. Here you do the opposite, defining it here and calling this operator in the *= one. I don't really have a preference but all the other operators are defined following the first rule so I would prefer avoiding this change for consistency.

there might also be some optimization under the hood doing it as it was as most of the operator overloading do it that way.

buschbapti

Thanks for changing the casts in pose there are still some left in twist and wrench

buschbapti · 2021-06-21T07:50:23Z

source/state_representation/src/space/cartesian/CartesianWrench.cpp

@@ -122,6 +140,10 @@ std::ostream& operator<<(std::ostream& os, const CartesianWrench& wrench) {
  return os;
 }

+CartesianWrench operator*(const CartesianState& state, const CartesianWrench& wrench) {
+  return CartesianWrench(state.operator*(wrench));


same as with pose which I have seen as now been corrected

Oups yeah sorry, early mornings....

buschbapti · 2021-06-21T07:50:51Z

source/state_representation/src/space/cartesian/CartesianTwist.cpp

@@ -149,6 +163,11 @@ Eigen::VectorXd CartesianTwist::data() const {
  return this->get_twist();
 }

+CartesianTwist CartesianTwist::inverse() const {
+  CartesianTwist result = this->CartesianState::inverse();
+  return result;


same as with pose which I have seen as now been corrected

buschbapti · 2021-06-21T07:50:53Z

source/state_representation/src/space/cartesian/CartesianTwist.cpp

@@ -164,6 +183,10 @@ std::ostream& operator<<(std::ostream& os, const CartesianTwist& twist) {
  return os;
 }

+CartesianTwist operator*(const CartesianState& state, const CartesianTwist& twist) {
+  return CartesianTwist(state.operator*(twist));


same as with pose which I have seen as now been corrected

domire8 · 2021-06-21T19:43:58Z

I will try to add a few more tests so maybe you can wait with your final reviwe 👍

eeberhard

I think this can pass for a 3.1 release, with the mentioned TODOs for the next release

domire8 requested review from buschbapti and eeberhard June 18, 2021 12:03

domire8 force-pushed the fix/cartesian-inverse branch from 4c91144 to 9dfe1ab Compare June 18, 2021 12:05

eeberhard reviewed Jun 18, 2021

View reviewed changes

eeberhard reviewed Jun 19, 2021

View reviewed changes

buschbapti reviewed Jun 20, 2021

View reviewed changes

domire8 force-pushed the fix/cartesian-inverse branch from 04cab11 to fa4bcf9 Compare June 21, 2021 06:54

buschbapti reviewed Jun 21, 2021

View reviewed changes

domire8 force-pushed the fix/cartesian-inverse branch from c2f8c32 to 078a3a5 Compare June 21, 2021 16:08

domire8 force-pushed the fix/cartesian-inverse branch from 078a3a5 to 98d7cc0 Compare June 22, 2021 16:12

eeberhard previously approved these changes Jul 2, 2021

View reviewed changes

Dominic Reber added 9 commits July 3, 2021 09:37

Remove the * and *= operators because they don't make sense

a016f7e

Explicitely define * operators and add inverse function

d3e31dc

Fix names

a5d16fb

Make operators deprecated and hide inverse functions

d75c356

Mark more operators as deprecated

bc1d463

Change the * and *= operator

da46390

Remove unnecessary casting

8cca183

*= operator should not be deprecated YET

97bd5f6

Update changelog

b8822f4

domire8 dismissed eeberhard’s stale review via b8822f4 July 3, 2021 07:38

domire8 force-pushed the fix/cartesian-inverse branch from d7b8977 to b8822f4 Compare July 3, 2021 07:38

eeberhard approved these changes Jul 3, 2021

View reviewed changes

domire8 merged commit a9531c8 into develop Jul 3, 2021

domire8 deleted the fix/cartesian-inverse branch July 3, 2021 08:04

Add inverse for CartesianPose and define operators explicitely #158

Add inverse for CartesianPose and define operators explicitely #158

Conversation

domire8 commented Jun 18, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

domire8 Jun 18, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

domire8 Jun 18, 2021 • edited Loading

Choose a reason for hiding this comment

domire8 commented Jun 18, 2021

domire8 commented Jun 19, 2021

eeberhard left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

buschbapti left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

buschbapti left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

domire8 commented Jun 21, 2021

eeberhard left a comment

Choose a reason for hiding this comment

domire8 Jun 18, 2021 •

edited

Loading

domire8 Jun 18, 2021 •

edited

Loading